/*
 * MxConversion.h
 *
 *  Created on: Oct 27, 2009
 *      Author: yamokosk
 */

#ifndef MXCONVERSION_H_
#define MXCONVERSION_H_

#include "../Vector3.h"
#include "../Quaternion.h"
#include "../Matrix4.h"

#include <mex.h>
#include <matrix.h>

namespace obrsp {

namespace linalg {

#define ACCESS_MX_ARRAY(pm,i,j,off) pm[(j)*off+(i)]
#define ACCESS_MX_ARRAY_44(pm,r,c) ACCESS_MX_ARRAY(pm,r,c,4)
#define ACCESS_MX_ARRAY_33(pm,r,c) ACCESS_MX_ARRAY(pm,r,c,3)

struct Vector3Mx
{
	static Vector3 convert(const mxArray* v)
	{
		const double* pv = mxGetPr(v);
		return Vector3((Real)pv[0], (Real)pv[1], (Real)pv[2]);
	}
};

struct QuaternionMx
{
	static Quaternion convert(const mxArray* v)
	{
		const double* pv = mxGetPr(v);
		return Quaternion((Real)pv[0], (Real)pv[1], (Real)pv[2], (Real)pv[3]);
	}
};

struct MxVector3
{
	static mxArray* convert(const Vector3& v)
	{
		mxArray* m = mxCreateDoubleMatrix( 3, 1, mxREAL );
		double* pm = mxGetPr(m);

		for (unsigned int n=0; n < 3; ++n)
		{
			pm[n] = (double)v[n];
		}

		return m;
	}
};

struct MxQuaternion
{
	static mxArray* convert(const Quaternion& q)
	{
		mxArray* m = mxCreateDoubleMatrix( 4, 1, mxREAL );
		double* pm = mxGetPr(m);

		for (unsigned int n=0; n < 4; ++n)
		{
			pm[n] = (double)q[n];
		}

		return m;
	}
};

struct MxMatrix3
{
	static mxArray* convert(const Matrix4& matrix)
	{
		mxArray* mxMatrix = mxCreateDoubleMatrix( 3, 3, mxREAL );
		double* pm = mxGetPr(mxMatrix);

		for (unsigned int r=0; r < 4; ++r)
		{
			for(unsigned int c=0; c <= 4; ++c)
			{
				ACCESS_MX_ARRAY_33(pm,r,c) = (double)matrix[r][c];
			}
		}

		return mxMatrix;
	}
};

struct MxMatrix4
{
	static mxArray* convert(const Matrix4& matrix)
	{
		mxArray* mxMatrix = mxCreateDoubleMatrix( 4, 4, mxREAL );
		double* pm = mxGetPr(mxMatrix);

		for (unsigned int r=0; r < 4; ++r)
		{
			for(unsigned int c=0; c <= 4; ++c)
			{
				ACCESS_MX_ARRAY_44(pm,r,c) = (double)matrix[r][c];
			}
		}

		return mxMatrix;
	}
};


}  // namespace linalg

}  // namespace obrsp

#endif /* MXCONVERSION_H_ */
